/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package stendcontrol;

import java.util.logging.Level;
import java.util.logging.Logger;
import services.ConvIntByte;

/**
 *
 * @author minaev
 */
public class StendControlAPI implements MessageListener {

    private UdpImplementer udp_imp;
    private SensorInformer sens_info;
    private CoefConfigurator c_coef = CoefConfigurator.getConf();
    private int wait_status = 0;
    private long wait_param = 0;
    private boolean waited = true;
    private int input_status = 0;
    private long input_param = 0;
    private boolean inputed = true;
    public static final int TYPE_RESEARCH_NONE = 0;
    public static final int TYPE_RESEARCH_DIST = 1;
    public static final int TYPE_RESEARCH_TIME = 2;
    long[] min_pos = new long[Configurator.COUNT_MOTORS];
    long[] max_pos = new long[Configurator.COUNT_MOTORS];
    private boolean restriction = false;
    private int current_engine = 0;

    public StendControlAPI(UdpImplementer udp_imp, SensorInformer sens_info) {
        this.udp_imp = udp_imp;
        this.sens_info = sens_info;
        for (int i = 0; i < Configurator.COUNT_MOTORS; i++) {
            min_pos[i] = CoefConfigurator.getConf().getMinPosition(i);
            max_pos[i] = CoefConfigurator.getConf().getMaxPosition(i);
        }
    }

    /**
     * Задать целевую позицию двигателя
     *
     * @param nmotor - номер двигателя
     * @param position - позиция двигателя
     */
    public void setTargetPosition(int nmotor, long position) {
        if (isValidPosition(position, nmotor)) {
            setCurrentEngine(nmotor);
            udp_imp.sendPacket(new CommandMessage(CommandMessage.TARGET_SENDER,
                    (byte) nmotor, position));
        } else {
            StendLogger.log(StendLogger.MSG_WARNING, "Motor num " + nmotor
                    + " target position out of range: " + position);
        }
    }

    public void setScaleTargetPosition(int nmotor, double position) {
        setTargetPosition(nmotor, SclVal.descale(position,
                c_coef.getPositionCoef0(nmotor), 
                c_coef.getPositionCoefProp(nmotor)));
    }

    /**
     * Ждать прихода в целевую позицию
     *
     * @param nmotor
     * @param position
     */
    public void waitTargetPosition(int nmotor, long position) {
        if (isValidPosition(position, nmotor)) {
            while (!waitStatusMessage((1 << 7) | nmotor, position)) {
                udp_imp.sendPacket(new CommandMessage(CommandMessage.GET_ENGINE_POS,
                        (byte) nmotor, 0));
            }
//        while (getPosition(nmotor)!=position) {
//        }
            try {
                Thread.sleep(20);
            } catch (InterruptedException ex) {
                Logger.getLogger(StendControlAPI.class.getName()).log(Level.SEVERE, null, ex);
            }
        } else {
            StendLogger.log(StendLogger.MSG_WARNING, "Motor num " + nmotor
                    + " wait position out of range: " + position);
        }
    }

    public void waitScaleTargetPosition(int nmotor, double position) {
        waitTargetPosition(nmotor, SclVal.descale(position,
                c_coef.getPositionCoef0(nmotor), 
                c_coef.getPositionCoefProp(nmotor)));
    }

    /**
     * Указать целевую позицию и ждать прихода в эту позицию
     *
     * @param nmotor - номер двигателя
     * @param position - целевая позиция
     */
    public void goToTargetPosition(int nmotor, long position) {
        setTargetPosition(nmotor, position);
        waitTargetPosition(nmotor, position);
    }

    public void goToScaleTargetPosition(int nmotor, double position) {
        goToTargetPosition(nmotor, SclVal.descale(position,
                c_coef.getPositionCoef0(nmotor), 
                c_coef.getPositionCoefProp(nmotor)));
    }

    public long getPosition(int nmotor) {
        udp_imp.sendPacket(new CommandMessage(CommandMessage.GET_ENGINE_POS,
                (byte) nmotor, 0));
        long ret;
        while ((ret = inputStatusMessage((1 << 7) | nmotor)) == Long.MIN_VALUE) {
            udp_imp.sendPacket(new CommandMessage(CommandMessage.GET_ENGINE_POS,
                    (byte) nmotor, 0));
        }
        /*System.out.println("position returned: " + ret
                + "(" + Long.toHexString(ret) + ")");*/
        return ret;
    }
    
    public double getScalePosition(int nmotor){
        return SclVal.scale(getPosition(nmotor), 
                c_coef.getPositionCoef0(nmotor), 
                c_coef.getPositionCoefProp(nmotor));
    }

    private synchronized long inputStatusMessage(int status) {
        input_status = status;
        inputed = false;
        try {
            this.wait(1000);
        } catch (InterruptedException ex) {
            StendLogger.log(StendLogger.MSG_ERROR, "Error in waitStatusMessage:"
                    + ex.getMessage());
        }
        return inputed ? this.input_param : Long.MIN_VALUE;
    }

    private synchronized boolean waitStatusMessage(int status, long param) {
        this.wait_status = status;
        this.wait_param = param;
        this.waited = false;
        //System.out.println("wait...");
        try {
            //System.out.println("Status  -> "  + status  + " : Param ->  "+ param );
            this.wait(1000);
        } catch (InterruptedException ex) {
            StendLogger.log(StendLogger.MSG_ERROR, "Error in waitStatusMessage:"
                    + ex.getMessage());
        }
        return waited;
    }

    /**
     * Получить неоткалиброванные данные с датчика
     *
     * @param nsensor - номер датчика
     * @return неоткалиброванные данные
     */
    public int getSensor(int nsensor) {
        return sens_info.getSensor(nsensor);
    }

    /**
     * Получить откалиброваные данные с датчика
     *
     * @param nsensor
     * @return
     */
    public double getScaleSensor(int nsensor) {
        return sens_info.getScaleSensor(nsensor);
    }

    /**
     * Установить скорость движения двигателей
     *
     * @param speed - скорость
     */
    public void setSpeed(int speed) {
        udp_imp.sendPacket(new CommandMessage(CommandMessage.SET_SPEED,
                ConvIntByte.int2Byte(speed), 0));
    }

    public void setCurrentEngine(int nmotor) {

        udp_imp.sendPacket(new CommandMessage(CommandMessage.SET_CUR_ENGINE,
                (byte) nmotor, 0));
        if (current_engine != nmotor) {
            current_engine = nmotor;
            try {
                Thread.sleep(200);
            } catch (InterruptedException ex) {
                Logger.getLogger(StendControlAPI.class.getName()).
                        log(Level.SEVERE, null, ex);
            }
        }

    }

    /**
     * Установить тип исследования (дискретизации) TYPE_RESEARCH_NONE - без
     * передачи данных TYPE_RESEARCH_TIME - дискретизация по времени
     * TYPE_RESEARCH_DIST - дискретизация по расстоянию
     *
     * @param rtype - тип исследования
     */
    public void setResearchType(int rtype) {
        udp_imp.sendPacket(new CommandMessage(CommandMessage.RESEARCH_TYPE,
                (byte) rtype, 0));
    }

    /**
     * Установить интервал дискретизации
     *
     * @param samp - интервал дискретизации
     */
    public void setSampleInterval(int samp) {
        udp_imp.sendPacket(new CommandMessage(CommandMessage.SET_SMPL_INTRV,
                ConvIntByte.int2Byte(samp), 0));
    }

    /**
     * Установить точность сенсора
     *
     * @param accuracy - true использовать точный
     */
    public void setSensorType(boolean accuracy) {
        if (accuracy) {
            udp_imp.sendPacket(new CommandMessage(
                    CommandMessage.SELECT_SENSOR_TYPE, (byte) 0, 0));
        } else {
            udp_imp.sendPacket(new CommandMessage(
                    CommandMessage.SELECT_SENSOR_TYPE, (byte) 1, 0));
        }
    }

    @Override
    public synchronized void msgBuilded(AbstractMessage msg) {
        if (msg.getTypeMsg() == AbstractMessage.MSG_STS) {
            if (!inputed && input_status == ((StatusMessage) msg).getStatus()) {
                input_param = ((StatusMessage) msg).getParam();
                inputed = true;
                notify();
            }
            if (!this.waited && wait_status == ((StatusMessage) msg).getStatus()
                    && wait_param == ((StatusMessage) msg).getParam()) {
                this.waited = true;
                notify();
            }
        }
    }

    public void setRestriction(boolean restriction) {
        this.restriction = restriction;
    }

    private boolean isValidPosition(long pos, int nmotor) {
        return (pos >= min_pos[nmotor] && pos <= max_pos[nmotor])
                || !restriction;
    }

    public int getCurrentEngine() {
        return current_engine;
    }
    
    
}
